#include <dwa_local_planner/apf_planner.h>
#include <cmath>
#include <math.h>
//for computing path distance
#include <queue>
#define MY_PI 3.1415926
//#include <angles/angles.h>
#include <dwa_local_planner/caltime.h>
#include <iostream> 
using std::ofstream;
namespace dwa_local_planner {

    APFPlanner::APFPlanner(std::string name,base_local_planner::LocalPlannerUtil *planner_util):planner_util_(planner_util)
    {
        ros::NodeHandle private_nh("~/" + name);
        private_nh.param("Attraction_K", Attraction_K, 30.0);
        private_nh.param("Repulsion_K", Repulsion_K, 15.0);
        private_nh.param("Obstacles_dis", Obstacles_dis, 0.6);
        private_nh.param("a", a, 0.5);

        private_nh.param("region_radius", region_radius, 0.8);
        private_nh.param("clear_radius", clear_radius, 0.4);
        private_nh.param("region_num", region_num, 36);
        private_nh.param("back_run_threshold", back_run_threshold, 0.2);
        private_nh.param("back_run_speed", back_run_speed, 0.1);
        private_nh.param("trun_speed", trun_speed, 0.2);
        private_nh.param("region_angle_turn", region_angle_turn, MY_PI/3);
        private_nh.param("region_angle_back", region_angle_back, 2*MY_PI/3);

        private_nh.param("shift_x", shift_x, 0.09);
        private_nh.param("shift_y", shift_y, 0.0);

        double control_frequency;
        private_nh.param("control_frequency", control_frequency, 5.0);

        private_nh.param("max_vel_x", max_vel_x, 0.4);
        private_nh.param("max_rot_vel", max_rot_vel, 0.6);
        private_nh.param("acc_lim_x", acc_lim_x, 0.4);
        private_nh.param("acc_lim_theta", acc_lim_theta, 0.8);

        private_nh.param("obstacle_range_short", obstacle_range_short, 0.4);
        private_nh.param("obstacle_range_long", obstacle_range_long, 0.5);
        
        if(control_frequency>1.0)
            sim_period_ = 1.0/control_frequency;
        else
            sim_period_ = 0.2;
        //std::cout<<"A_K:"<<Attraction_K<<",R_K:"<<Repulsion_K<<std::endl;

        int cx = 10, cy = 10;
        p_calc_ = new global_planner::PotentialCalculator(cx, cy);
        anyangle_apf_planner_ = new global_planner::AnyAngleExpansion(p_calc_, cx, cy);
        anyangle_apf_planner_->setFilledPath();
        apf_num_x = 2.0 / planner_util_->getCostmap()->getResolution() ;
        apf_num_y = 2.0 / planner_util_->getCostmap()->getResolution() ;
    }

    void APFPlanner::apfInit(double start_point[3], double goal_point[2], std::vector<std::vector <double> > obstacles)
    {
        this->Attraction_K=Attraction_K;
        this->Repulsion_K=Repulsion_K;
        this->Obstacles_dis=Obstacles_dis;
        this->a=a;
        this->start_point[0]=start_point[0];
        this->start_point[1]=start_point[1];
        this->start_point[2]=start_point[2];
        set_position_to_center();
        this->goal_point[0]=goal_point[0];
        this->goal_point[1]=goal_point[1];
        this->obstacles=obstacles;
        this->obs_num = obstacles.size();
        map = planner_util_->getCostmap()->getCharMap();
    }
    void APFPlanner::set_position_to_center()
    {
        start_center[0] = start_point[0] - shift_x * std::cos(start_point[2]);
                          //  planner_util_->getCostmap()->getResolution();
        start_center[1] = start_point[1] - shift_x * std::sin(start_point[2]);
                          //  planner_util_->getCostmap()->getResolution();
    }
    std::vector<double> APFPlanner::find_nearest_obstacle()
    {
        double min_dis = 1e6;
        double pow_dis;
        int index = -1;

        for(int i=0;i<obstacles.size();i++)
        {   
            double angle_obs = atan2(obstacles[i][1]-start_center[1],\
                        obstacles[i][0]-start_center[0]) - start_point[2];
            if(fabs(angle_obs) > 1.57)  continue;
            pow_dis = std::pow(obstacles[i][0]-start_center[0],2)+\
                        std::pow(obstacles[i][1]-start_center[1],2);
            if(pow_dis < min_dis)
            {
                min_dis = pow_dis;
                index = i;
            }
        }

        std::vector<double> null_obs;
        if(index == -1) return null_obs;
        else   return obstacles[index];

    }
    std::vector<double> APFPlanner::computeAngle()
    {
        std::vector<double> Y;
        double deltax,deltay,r;
        for(int i=0;i<obs_num+1;i++)
        {
            if(i!=0)
            {
                deltax=this->obstacles[i-1][0]-start_point[0];
                deltay=this->obstacles[i-1][1]-start_point[1];
            }
            else
            {
                deltax=this->goal_point[0]-start_point[0];
                deltay=this->goal_point[1]-start_point[1];
            }
            r=sqrt(deltax*deltax+deltay*deltay);
            if(deltay>0)
                Y.push_back(acos(deltax/r));
            else
                Y.push_back(-acos(deltax/r));
        }
        return Y;
    }
    std::vector<double> APFPlanner::computeAttraction(std::vector<double> att_angle)
    {
        double R=(goal_point[0]-start_point[0])*(goal_point[0]-start_point[0])\
            +(goal_point[1]-start_point[1])*(goal_point[1]-start_point[1]);
        double r=sqrt(R);
        std::vector<double> Yatt;
        Yatt.resize(2);
        Yatt[0]=Attraction_K*r*cos(att_angle[0]);
        Yatt[1]=Attraction_K*r*sin(att_angle[0]);
        return Yatt;
    }
    std::vector<double> APFPlanner::computeRepulsion(std::vector<double> angle)
    {
        std::vector<double> YY;
        double Rat=(start_point[0]-goal_point[0])*(start_point[0]-goal_point[0])\
                        +(start_point[1]-goal_point[1])*(start_point[1]-goal_point[1]);
        double rat=sqrt(Rat);
        double Rre,rre,Yrer,Yata;
        std::vector<double> Yrerx, Yrery, Yatax, Yatay;
        Yrerx.resize(obs_num);
        Yrery.resize(obs_num);
        Yatax.resize(obs_num);
        Yatay.resize(obs_num);
        for(int i=0;i<obs_num;i++)
        {
            Rre=(start_point[0]-obstacles[i][0])*(start_point[0]-obstacles[i][0])\
                    +(start_point[1]-obstacles[i][1])*(start_point[1]-obstacles[i][1]);
            rre=sqrt(Rre);
            if(rre>Obstacles_dis)
            {
                Yrerx[i]=0;
                Yrery[i]=0;
                Yatax[i]=0;
                Yatay[i]=0;
            }
            else if(rre>Obstacles_dis/2)
            {
                Yrer=Repulsion_K*(1/rre-1/Obstacles_dis)*(1/Rre)*Rat;//分解的Fre1向量
                Yata=Repulsion_K*((1/rre-1/Obstacles_dis)*(1/rre-1/Obstacles_dis))*rat;//分解的Fre2向量       Yata(i)=0;
                Yrerx[i]=Yrer*cos(angle[i+1]+3.1415 );  //angle_re(i)=Y(i+1)
                Yrery[i]=Yrer*sin(angle[i+1]+3.1415 );
                Yatax[i]=Yata*cos(angle[0]);            //angle_at=Y(1)
                Yatay[i]=Yata*sin(angle[0]);
            }
            else if(rre<Obstacles_dis/2)
            {
                Yrer=Repulsion_K*(1/rre-1/Obstacles_dis)*(1/Rre)*(pow(rat,a));//分解的Fre1向量
                Yata=a*Repulsion_K*((1/rre-1/Obstacles_dis)*(1/rre-1/Obstacles_dis))*(pow(rat,(1-a)))/2;//分解的Fre2向量   Yata(i)=0;
                Yrerx[i]=Yrer*cos(angle[i+1]+3.1415);  //angle_re(i)=Y(i+1)
                Yrery[i]=Yrer*sin(angle[i+1]+3.1415);
                Yatax[i]=Yata*cos(angle[0]);            //angle_at=Y(1)
                Yatay[i]=Yata*sin(angle[0]);
            }
        }
        YY.push_back(sum(Yrerx));
        YY.push_back(sum(Yrery));
        YY.push_back(sum(Yatax));
        YY.push_back(sum(Yatay));
        return YY;
    }
    double APFPlanner::computePotential()
    {
        double sum_potential = 0;
        double Rre,rre,cur_potential;
        for(int i=0;i<obs_num;i++)
        {
            Rre=(start_point[0]-obstacles[i][0])*(start_point[0]-obstacles[i][0])\
                    +(start_point[1]-obstacles[i][1])*(start_point[1]-obstacles[i][1]);
            rre=sqrt(Rre);
            
            if(rre > Obstacles_dis)
                cur_potential = 0.5*Repulsion_K*pow(1/(rre)-1/Obstacles_dis,2);
            else
                cur_potential = 0.0;
            sum_potential += cur_potential;
        }
        return sum_potential;
    }
    double APFPlanner::sum(std::vector<double> p)
    {
        double sum=0;
        for(int i=0;i<obs_num;i++)
        {
            sum+=p[i];
        }
        return sum;
    }
    float APFPlanner::angle2_pi_pi(float angle)
    {
        while(angle<=-MY_PI)
        angle += 2*MY_PI;
        while(angle>MY_PI)
        angle -= 2*MY_PI;
        return  angle;
    }
    
    bool APFPlanner::regionSegmention()
    {
        region_front.clear();
        region_left.clear();
        region_right.clear();
        region_back.clear();
        double step_angle = 2*MY_PI/region_num;
        std::vector<Region_> region;
        unsigned int start_x_cell,start_y_cell;
        region.resize(region_num);
        if(!planner_util_->getCostmap()->worldToMap(start_point[0], start_point[1], start_x_cell,  start_y_cell)){
            ROS_ERROR("start point exceed local map ...");
            return false;
        }
        for(int i=0; i<region_num;i++)
        { 
            double angle = angle2_pi_pi(step_angle*i) - start_point[2];
            angle = angle2_pi_pi(angle);

            double x1 = start_center[0] + region_radius*cos(step_angle*i);
            double y1 = start_center[1] + region_radius*sin(step_angle*i);

            double x2 = start_center[0] + clear_radius*cos(step_angle*i);
            double y2 = start_center[1] + clear_radius*sin(step_angle*i);
            unsigned int vertex1_x_cell,vertex1_y_cell;
            unsigned int vertex2_x_cell,vertex2_y_cell;
            if(!planner_util_->getCostmap()->worldToMap(x1, y1, vertex1_x_cell, vertex1_y_cell)){
                ROS_ERROR("vertex point exceed local map ...");
                return false;
            }
            if(!planner_util_->getCostmap()->worldToMap(x2, y2, vertex2_x_cell, vertex2_y_cell)){
                ROS_ERROR("vertex point exceed local map ...");
                return false;
            }
            std::vector<int> line_x,line_y;
            anyangle_apf_planner_->bresenham(vertex1_x_cell,vertex1_y_cell,vertex2_x_cell,vertex2_y_cell,line_x,line_y);
            region[i].vertex.resize(line_x.size());
            for(int j=0;j<line_x.size();j++)
            {
                region[i].vertex[j].x = (unsigned int)(line_x[j]);
                region[i].vertex[j].y = (unsigned int)(line_y[j]);
            }
            region[i].region_angle = angle;
//            ROS_INFO("bre num:%d",line_x.size());
        }
        std::vector<costmap_2d::MapLocation> vertex_first = region[0].vertex;
        for(int i=0;i<region_num-1;i++)
        {
            region[i].vertex.insert(region[i].vertex.end(),
                                    region[i+1].vertex.begin(),
                                    region[i+1].vertex.end());
        }
        region[region_num-1].vertex.insert(region[region_num-1].vertex.end(),\
                                           vertex_first.begin(),
                                           vertex_first.end());
        ofstream outfile_front,outfile_left,outfile_right,outfile_back;

        for(int i=0;i<region_num;i++)
        {
//            ROS_INFO("regin vertex num:%d",region[i].vertex.size());
            if(std::fabs(region[i].region_angle) <= region_angle_turn)
            {
                region_front.push_back(region[i]);  
            }
            else if(region[i].region_angle > region_angle_turn && \
                region[i].region_angle <= region_angle_back)
            {
                region_left.push_back(region[i]);
            }
            else if(region[i].region_angle >= -region_angle_back && \
                region[i].region_angle < -region_angle_turn)
            {
                region_right.push_back(region[i]);
            }
            else
            {
                region_back.push_back(region[i]);
            }            
        }
        return true;
    }

    bool APFPlanner::getRegionPoint()
    {
        for(int i=0;i<region_front.size();i++)
        {
            std::vector<costmap_2d::MapLocation> polygon_cells;
            planner_util_->getCostmap()->convexFillCells(region_front[i].vertex, polygon_cells);
            region_front[i].vertex = polygon_cells;
        }
        for(int i =0;i<region_left.size();i++)
        {
            std::vector<costmap_2d::MapLocation> polygon_cells;
            planner_util_->getCostmap()->convexFillCells(region_left[i].vertex, polygon_cells);
            region_left[i].vertex = polygon_cells;
        } 
        for(int i =0;i<region_right.size();i++)
        {
            std::vector<costmap_2d::MapLocation> polygon_cells;
            planner_util_->getCostmap()->convexFillCells(region_right[i].vertex, polygon_cells);
            region_right[i].vertex = polygon_cells;
        }
        for(int i =0;i<region_back.size();i++)
        {
            std::vector<costmap_2d::MapLocation> polygon_cells;
            planner_util_->getCostmap()->convexFillCells(region_back[i].vertex, polygon_cells);
            region_back[i].vertex = polygon_cells;
        }        
    }

    bool APFPlanner::caculateOccupiedGrid(std::vector<costmap_2d::MapLocation> &polygon)
    {
        int nx = planner_util_->getCostmap()->getSizeInCellsX();
        int ny = planner_util_->getCostmap()->getSizeInCellsY();
        for(int i=0;i<polygon.size();i++)
        {
            if(map[nx*polygon[i].y + polygon[i].x] >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
            {
                return false;
            }
        }
        return true;
    }
 
    bool APFPlanner::judgeDirection(float &angle, DirState dir)
    {
        std::vector<Region_> region_candidate;
        switch(dir)
        {
            case FRONT:
                region_candidate = region_front;
                break;
            case LEFT:
                region_candidate = region_left;
                break;
            case RIGHT:
                region_candidate = region_right;
                break;
            case BACK:
                region_candidate = region_back;
                break;
            default:
                break;
        }
        for(int i=0;i<region_candidate.size();i++)
        {
            if(caculateOccupiedGrid(region_candidate[i].vertex))
                region_candidate[i].accessable = true;
            else
                region_candidate[i].accessable = false;
        }
        bool dir_valid = false;
        double min_det_angle = MY_PI;
        int best_index;
        vector<vector<int> > free_areas;
        vector<int> valid_num;
        vector<int> free_area;
        int cnt = 0;
        bool last_free_add = true;
        for(int i=0;i<region_candidate.size();i++)
        {
            if(region_candidate[i].accessable)
            {
                free_area.push_back(i);
                cnt++;
                last_free_add = true;
            }
            else
            {
                last_free_add = false;
                if(free_area.size()>0)
                {
                    free_areas.push_back(free_area);
                    free_area.clear();
                    valid_num.push_back(cnt);
                    cnt = 0;
                }
            }            
        }
        if(last_free_add)
        {
            free_areas.push_back(free_area);
            free_area.clear();
            valid_num.push_back(cnt);
            cnt = 0;
        }

        if(free_areas.size() == 0)  return false;

        vector<double> angles;
        angles.resize(free_areas.size());

        for(int i=0;i<free_areas.size();i++)
        {
            int first_index = free_areas[i][0];
            double last_angle = region_candidate[first_index].region_angle;
            double angle = last_angle;
            for(int j=1;j<free_areas[i].size();j++)
            {
                int index = free_areas[i][j];
                double temp = region_candidate[index].region_angle;
                if(temp > last_angle)
                {
                    if(temp-last_angle > MY_PI)
                        temp -= 2*MY_PI;
                }
                else
                {
                    if(temp-last_angle < -MY_PI)
                        temp += 2*MY_PI;
                }
                last_angle = temp;
                angle += temp;
            }
            angles[i] = angle2_pi_pi(angle/free_areas[i].size());
        }
        
        best_index = 0;
        for(int i=1;i<valid_num.size();i++)
        {
            if(valid_num[best_index] < valid_num[i])
                best_index = i;
        }
        angle = angles[best_index];

        return true;
    }
 
    void APFPlanner::backRun(float angle, tf::Stamped<tf::Pose>& drive_velocities)
    {
        double vth;
        double time = back_run_threshold / back_run_speed;
        if(angle>0)
        {
            vth = (angle-MY_PI)/time;
            vth = vth > -max_rot_vel ? vth : -max_rot_vel;
        }
        else
        {
            vth = (angle+MY_PI)/time;
            vth = vth < max_rot_vel ? vth : max_rot_vel;
        }

//        vth = 0.0;                        
        setSpeed(-back_run_speed,0.0,vth);
        drive_velocities = drive_velocities_;
    }

    void APFPlanner::setSpeedSmooth(float vx, float vth)
    {
        double mv_x,mv_th;
        if(vx < current_vel[0])
            mv_x = current_vel[0] - sim_period_*acc_lim_x > vx ? current_vel[0]-sim_period_*acc_lim_x : vx;
        else 
            mv_x = current_vel[0] + sim_period_*acc_lim_x < vx ? current_vel[0]+sim_period_*acc_lim_x : vx;

        if(current_vel[2] > vth)
            mv_th = current_vel[2] - sim_period_*acc_lim_theta > vth ? current_vel[2] - sim_period_*acc_lim_theta : vth;
        else 
            mv_th = current_vel[2] + sim_period_*acc_lim_theta < vth ? current_vel[2] + sim_period_*acc_lim_theta : vth;

        setSpeed(mv_x,0.0,mv_th);
    }

    void APFPlanner::setSpeed(float vx, float vy, float vth)
    {
        tf::Vector3 start(vx, vy, 0);
        drive_velocities_.setOrigin(start);
        tf::Matrix3x3 matrix;
        matrix.setRotation(tf::createQuaternionFromYaw(vth));
        drive_velocities_.setBasis(matrix);
    }
    void APFPlanner::setSpeedSmooth(  
        float vx,float vth,      
        tf::Stamped<tf::Pose> global_vel,
        tf::Stamped<tf::Pose>& drive_velocities)
    {
        current_vel[0] = global_vel.getOrigin().getX();
        current_vel[1] = global_vel.getOrigin().getY();
        current_vel[2] = tf::getYaw(global_vel.getRotation());
        setSpeedSmooth(vx,vth);
        drive_velocities = drive_velocities_;
    }

    bool APFPlanner::back_run_some_dis(double back_angle, tf::Stamped<tf::Pose>&drive_velocities)
    {
        static ros::Time time = ros::Time::now();
        static ros::Time last_time = time;
        static bool fisrt_back_run = true;
        static double start_position[2] = {start_point[0],start_point[1]};
        time = ros::Time::now();

        if(time.toSec() + 0.0001 -last_time.toSec()< sim_period_*5.0)   //keep back run
        {
            last_time = time;
        }
        else                                                            //restart back run
        {
            ROS_INFO("back_run_some_dis back run flag first");
            last_time = time;
            fisrt_back_run = true;
        }

        if(fisrt_back_run)
        {
            fisrt_back_run = false;
            
            //set start info
            start_position[0] = start_point[0];
            start_position[1] = start_point[1];

            //back run;
            ROS_INFO("back_run_some_dis first run back");
            backRun(back_angle,drive_velocities);
            return false;
        }
        else{
            double distance = pow(start_point[0]-start_position[0],2) + \
                                pow(start_point[1]-start_position[1],2);
            if(distance>back_run_threshold*back_run_threshold)      //back run so far, stop
            {
                ROS_WARN("back run for enough distance");
                setSpeedSmooth(0.,0.);
                drive_velocities = drive_velocities_;
                return false;
            }                                                      
            else                                                    //back run
            {
                ROS_INFO("back_run_some_dis run back");
                backRun(back_angle,drive_velocities);
                return true;
            }                    
        }
    }

    void APFPlanner::findBestPath(
        tf::Stamped<tf::Pose> global_pose,
        tf::Stamped<tf::Pose> global_vel,
        tf::Stamped<tf::Pose>& drive_velocities,
        std::vector<geometry_msgs::Point> footprint_spec) 
    {
        double vel[3] = {global_vel.getOrigin().getX(),\
                        global_vel.getOrigin().getY(),\
                        tf::getYaw(global_vel.getRotation())};
        double pos[3] = {global_pose.getOrigin().getX(),\
                        global_pose.getOrigin().getY(), \
                        tf::getYaw(global_pose.getRotation())};

        current_vel[0] = global_vel.getOrigin().getX();
        current_vel[1] = global_vel.getOrigin().getY();
        current_vel[2] = tf::getYaw(global_vel.getRotation());

        std::vector<double> angle_re = computeAngle();
        std::vector<double> Yatt = computeAttraction(angle_re);
        std::vector<double> Y = computeRepulsion(angle_re);

        double Fsumyj=Yatt[1]+Y[1]+Y[3];
        double Fsumxj=Yatt[0]+Y[0]+Y[2];
        double Position_angle=std::atan2(Fsumyj,Fsumxj);

        double vth = vel[2];
        double apf_angle = angle2_pi_pi(Position_angle - pos[2]);

        float path_angle = 0;    
        float fusion_angle = apf_angle;

        //dierection judge
        float front_angle,left_angle,right_angle,back_angle;
        if(true)
        {
            Caltime cal_time;
            cal_time.set_start_time();
            static double back_run_distance = 0;
            if(!regionSegmention())                         //something error, stop
            {
                setSpeedSmooth(0.,0.);   
                drive_velocities = drive_velocities_;
                return ;
            }

            cal_time.set_stop_time();
            if(judgeDirection(front_angle,FRONT))
            {
                cal_time.set_stop_time();
            }
            else if(judgeDirection(left_angle,LEFT))
            {
                setSpeedSmooth(0.0,trun_speed);
                drive_velocities = drive_velocities_;
               
                return ;
            }
            else if(judgeDirection(right_angle,RIGHT))
            {
                setSpeedSmooth(0.0,-trun_speed);
                drive_velocities = drive_velocities_;
                return ;
            }
            else if(judgeDirection(back_angle,BACK))
            {
                //cal_time.set_stop_time();
                back_run_some_dis(back_angle,drive_velocities);
                return ;
            }
            else
            {
		        ROS_WARN("can not move");
                setSpeedSmooth(0.,0.);
                drive_velocities = drive_velocities_;      //cannot move, stop or turn circle
                return ;
            }           
        }

        
        double expectVel = vel[0];
        if(fabs(fusion_angle)>MY_PI/2)
        {
            expectVel = vel[0] - 0.03*sim_period_/0.2;
        }
	    expectVel = std::min(expectVel,0.5);    //0.3
        expectVel = std::max(expectVel,0.2);
        if(fusion_angle>0. && fusion_angle < 0.2)
        {
            vth = vel[2]+0.005*sim_period_/0.2;
        }
        else if(fusion_angle>0.2 && fusion_angle < MY_PI/2)
        {
            vth = vel[2]+0.01*sim_period_/0.2;
        }
        else if(fusion_angle<MY_PI && fusion_angle > MY_PI/2)
        {
            vth = vel[2]+0.02*sim_period_/0.2;
        }
        else if(fusion_angle<0. && fusion_angle > -0.2)
        {
            vth = vel[2]-0.005*sim_period_/0.2;
        }
        else if(fusion_angle<-0.2 && fusion_angle > -MY_PI/2)
        {
            vth = vel[2]-0.01*sim_period_/0.2;
        }
        else
        {
            vth = vel[2]-0.02*sim_period_/0.2;
        }
        if(fusion_angle > 0.)
        {
            vth = std::min(0.4,vth);
            vth = std::max(0.0,vth);
        }
        else
        {
            vth = std::min(0.0,vth);
            vth = std::max(-0.4,vth);
        }
        vth = std::min(vth,0.4);
        vth = std::max(vth,-0.4);
        tf::Vector3 start(expectVel, 0, 0);
        drive_velocities.setOrigin(start);
        tf::Matrix3x3 matrix;
        matrix.setRotation(tf::createQuaternionFromYaw(vth));
        drive_velocities.setBasis(matrix);
    }
    bool APFPlanner::avoidAction(tf::Stamped<tf::Pose>& drive_velocities,float angle)
    {
        if(fabs(angle)>MY_PI/5)     //trun
        {
            if(angle > 0)
            {
                setSpeedSmooth(0.0,trun_speed);
                drive_velocities = drive_velocities_;
                ROS_INFO("turn to left");
            }
            else
            {
                setSpeedSmooth(0.0,-trun_speed);
                drive_velocities = drive_velocities_;
                ROS_INFO("turn to right");
            }
            return true;            
        }
        else
        {
            float back_angle = MY_PI;
            if(!judgeDirection(back_angle,BACK))    //can not run back, stop move
            {
                setSpeedSmooth(0.0,0.0);
                drive_velocities = drive_velocities_;
                ROS_INFO("can not run back");
                return false;
            }
            else                                    //back run, stop if back run max distance
            {
                return back_run_some_dis(back_angle,drive_velocities);
            }
        }
    }
    bool APFPlanner::need_avoid(bool expand_flag)

    {
        double expand = 0.0;
        if(expand_flag) 
            expand = 0.1;
        double pow_dis;
        double obstacle_range_avr = obstacle_range_short*0.5+obstacle_range_long*0.5;
        for(int i=0;i<obstacles.size();i++)
        {   
            double angle_obs = atan2(obstacles[i][1]-start_center[1],\
                        obstacles[i][0]-start_center[0]) - start_point[2];
            
            if(fabs(angle_obs) > 1.57)  
                continue;

            pow_dis = std::pow(obstacles[i][0]-start_center[0],2)+\
                        std::pow(obstacles[i][1]-start_center[1],2);
            if(fabs(angle_obs)<0.523)//30 degree
            {
                if(pow_dis<pow(obstacle_range_long,2))
                    return true;
            }
            else if(fabs(angle_obs)<1.046)//60 degree
            {
                if(pow_dis<pow(obstacle_range_avr,2))
                    return true;
            }
            else
            {
                if(pow_dis<pow(obstacle_range_short,2))
                    return true;
            }
        }
        return false;
    }
    bool APFPlanner::backRunDetect(
        tf::Stamped<tf::Pose> global_vel,
        tf::Stamped<tf::Pose>& drive_velocities) 
    {
        current_vel[0] = global_vel.getOrigin().getX();
        current_vel[1] = global_vel.getOrigin().getY();
        current_vel[2] = tf::getYaw(global_vel.getRotation());
        if(!regionSegmention())                         //something error, stop
        {
            setSpeedSmooth(0.,0.);   
            drive_velocities = drive_velocities_;
            return true;
        }

        std::vector<double> potential_obstacle = find_nearest_obstacle();
        if(potential_obstacle.empty())
            return false;
        double dis_obs = pow(potential_obstacle[0]-start_point[0],2)+\
            pow(potential_obstacle[1]-start_point[1],2);
        double angle_obs = std::atan2(potential_obstacle[1]-start_point[1],\
            potential_obstacle[0]-start_point[0]) - start_point[2];

//        ROS_INFO("near obstacle:%f",sqrt(dis_obs));
        bool need_avoid_now_flag;
        float front_angle ,left_angle,back_angle,right_angle;
        if(!avoid_flag)     
        {
            need_avoid_now_flag = need_avoid(false);
        }
        else                //avoid state
        {
            need_avoid_now_flag = need_avoid(true);
        }

        if(!need_avoid_now_flag)
        {
            avoid_flag = false;
//            ROS_INFO("no need avoid action");
            return false;
        }
        else
        {
            avoid_flag = true;
            avoidAction(drive_velocities, angle_obs);
            ROS_WARN("avoid action");
            return true;
        }
    }
    bool APFPlanner::iaPathClear()
    {
        unsigned int vertex1_x_cell,vertex1_y_cell;
        unsigned int vertex2_x_cell,vertex2_y_cell;
        int nx  = planner_util_->getCostmap()->getSizeInCellsX();
        int ny = planner_util_->getCostmap()->getSizeInCellsY(); 
        if(!planner_util_->getCostmap()->worldToMap(start_point[0], start_point[1], vertex1_x_cell, vertex1_y_cell)){
            ROS_ERROR("vertex point exceed local map ...");
            return false;
        }
        if(!planner_util_->getCostmap()->worldToMap(goal_point[0], goal_point[1], vertex2_x_cell, vertex2_y_cell)){
            ROS_ERROR("vertex point exceed local map ...");
            return false;
        }
        std::vector<int> line_x,line_y;
        anyangle_apf_planner_->bresenham(vertex1_x_cell,vertex1_y_cell,vertex2_x_cell,vertex2_y_cell,line_x,line_y);
        for(int i=0;i<line_x.size();i++)
        {
            if(map[line_y[i]*nx+line_x[i]] > costmap_2d::FREE_SPACE)
                return false;
        }
        return true;
    } 
    bool APFPlanner::parking(
        tf::Stamped<tf::Pose> global_pose,
        tf::Stamped<tf::Pose> global_vel,
        tf::Stamped<tf::Pose>& drive_velocities,
        std::vector<geometry_msgs::Point> footprint_spec) 
    {
        double vel[3] = {global_vel.getOrigin().getX(),\
                        global_vel.getOrigin().getY(),\
                        tf::getYaw(global_vel.getRotation())};
        double pos[3] = {global_pose.getOrigin().getX(),\
                        global_pose.getOrigin().getY(), \
                        tf::getYaw(global_pose.getRotation())};
        //ROS_INFO("parking ...");
        current_vel[0] = global_vel.getOrigin().getX();
        current_vel[1] = global_vel.getOrigin().getY();
        current_vel[2] = tf::getYaw(global_vel.getRotation());

        double start_to_goal = atan2(goal_point[1]-start_point[1],goal_point[0]- start_point[0]);
        double robot_to_goal = angle2_pi_pi(start_to_goal - pos[2]);
        double dis = sqrt(pow(goal_point[1]-start_point[1],2)+pow(goal_point[0]- start_point[0],2));
        if(std::abs(robot_to_goal)<2.88 && dis > 0.30)    //15 dgree : 180-15
        {
            if(robot_to_goal > 0)           //turn right
            {
                setSpeedSmooth(0.0,-0.3);
                drive_velocities = drive_velocities_;
            }
            else                            //turn left
            {
                setSpeedSmooth(0.0,0.3);
                drive_velocities = drive_velocities_;
            }            
        }
        else
        {
            if(iaPathClear())
            {
                double parking_speed = 0.20;
                
                double t = dis / parking_speed;
                if (t < 1.0) t = 1.0; 
                double vth = -robot_to_goal / t;
                double  vth_threshold = 0.2;
                if(vth>0)
                    vth = vth < vth_threshold ? vth : vth_threshold;
                else
                    vth = vth > -vth_threshold ? vth : -vth_threshold;

                setSpeedSmooth(-parking_speed,vth);
                drive_velocities = drive_velocities_;
            }
            else
            {
                //ROS_WARN("cannot move , stop ");
                setSpeedSmooth(0.0,0.0);
                drive_velocities = drive_velocities_;
            }
        }   
    }
};
